/*
 * @Description: 后端具体实现功能
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:05:52
 * @LastEditTime: 2021-11-11 14:50:43
 * @LastEditors: Sang Hao
 */

#include <Eigen/Dense>
#include <pcl/io/pcd_io.h>
#include "glog/logging.h"

#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/mapping/back_end/back_end.hpp"
#include "lidar_slam/tools/file_manager.hpp"

namespace lidar_slam {

BackEnd::BackEnd() {
	InitWithConfig();
}

bool BackEnd::InitWithConfig() {
	std::string config_file_path = WORK_SPACE_PATH + "/config/back_end.yaml";
	std::cout << "后端配置文件路径：" << config_file_path << std::endl;
	YAML::Node config_node = YAML::LoadFile(config_file_path);

	std::cout << "----------------后端初始化---------------" << std::endl;
	InitParam(config_node);
	InitGraphOptimizer(config_node);
	InitDataPath(config_node);

	return true;
}

bool BackEnd::InitParam(const YAML::Node& config_node) {
	key_frame_distance_ = config_node["key_frame_distance"].as<float>();

	return true;
}

/**
 * @description: 构造优化的求解器
 * @param  {*}
 * @return {*}
 */
bool BackEnd::InitGraphOptimizer(const YAML::Node& config_node) {
	std::string graph_optimizer_type = config_node["graph_optimizer_type"].as<std::string>();
	if (graph_optimizer_type == "g2o") {
		graph_optimizer_ptr_ = std::make_shared<G2oGraphOptimizer>("lm_var");
	} else {
		std::cout << "没有找到合适的优化方法" << std::endl;
		return false;
	}
	std::cout << "后端选择的优化器为：" << graph_optimizer_type << std::endl;

	graph_optimizer_config_.use_loop_close = config_node["use_loop_close"].as<bool>();
	std::cout << "优化器是否考虑使用闭环(use_loop_close)：" << graph_optimizer_config_.use_loop_close << std::endl;
	/* 这一行代码有问题，可是问题在哪里呢？ */
	graph_optimizer_config_.optimize_step_with_key_frame = config_node["optimize_step_with_key_frame"].as<int>();
	graph_optimizer_config_.optimize_step_with_loop = config_node["optimize_step_with_loop"].as<int>();

	for (int i = 0; i < 6; ++i) {
		graph_optimizer_config_.odom_edge_noise(i) = 
			config_node[graph_optimizer_type + "_param"]["odom_edge_noise"][i].as<double>();
		graph_optimizer_config_.close_loop_noise(i) = 
			config_node[graph_optimizer_type + "_param"]["close_loop_noise"][i].as<double>();

	}

	// for (int i = 0; i < 3; ++i) {
	// 	graph_optimizer_config_.gnss_noise(i) = 
	// 		config_node[graph_optimizer_type + "_param"]["gnss_noise"][i].as<double>();

	// }

	return true;
}

bool BackEnd::InitDataPath(const YAML::Node& config_node) {
	std::string data_path = config_node["data_path"].as<std::string>();
	if (data_path == "./") {
		data_path = WORK_SPACE_PATH;
	}

	/* 创建slam_data文件夹，FileManager不能跳级创建文件夹 */
	/* 只要创建成功，或者本来就有这个文件夹，则会返回true，不会进到if中 */
	if (!FileManager::CreateDirectory(data_path + "/slam_data")) {
		return false;
	}

	key_frames_path_ = data_path + "/slam_data/key_frames";
	trajectory_path_ = data_path + "/slam_data/trajectory";
	
	if (!FileManager::InitDirectory(key_frames_path_, "关键帧点云")) {
		return false;
	}
	if (!FileManager::InitDirectory(trajectory_path_, "轨迹文件")) {
		return false;
	}

	if (!FileManager::CreateFile(ground_truth_ofs_, trajectory_path_ + "/ground_truth.txt")) {
		return false;
	}
	if (!FileManager::CreateFile(laser_odom_ofs_, trajectory_path_ + "/laser_odom.txt")) {
		return false;
	}

	return true;
}


void BackEnd::ResetParam() {
	has_new_key_frame_ = false;
	has_new_optimized_ = false;
}

/**
 * @description: 将位姿保存到std::ofstream中，后续可转为文件等
 * @param  {*}
 * @return {*}
 */
bool BackEnd::SavePose(std::ofstream& ofs, const Eigen::Matrix4f& pose) {
	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 4; ++j) {
			ofs << pose(i, j);

			if (i == 2 && j == 3) {
				ofs << std::endl;
			} else {
				ofs << " ";
			}
		}
	}

	return true;
}

bool BackEnd::AddNodeAndEdge() {
	Eigen::Isometry3d isometry;
	/* 添加关键帧节点 */
	isometry.matrix() = current_key_frame_.pose.cast<double>();
	if (graph_optimizer_ptr_->GetNodeNum() == 0) {
		graph_optimizer_ptr_->AddSe3Node(isometry, true);
	} else {
		graph_optimizer_ptr_->AddSe3Node(isometry, false);
	}
	new_key_frame_cnt_ ++;

	/* 添加激光里程计对应的边 */
	static KeyFrame last_key_frame = current_key_frame_;
	int node_num = graph_optimizer_ptr_->GetNodeNum();
	if (node_num > 1) {
		Eigen::Matrix4f relative_pose = last_key_frame.pose.inverse() * current_key_frame_.pose;
		isometry.matrix() = relative_pose.cast<double>();
		/* 添加边，两个节点index，isometry，和优化噪声参数 */
		graph_optimizer_ptr_->AddSe3Edge(node_num - 2, node_num - 1, isometry, graph_optimizer_config_.odom_edge_noise);

	}
	last_key_frame = current_key_frame_;

	/* 添加GNSS位置对应的先验边，这里没有gnss */
	/* 这里边调用AddSe3PriorXYZEdge */

	return true;
}

bool BackEnd::MaybeNewKeyFrame(const CloudData& cloud_data, const PoseData& laser_odom) {
	static Eigen::Matrix4f last_key_pose = laser_odom.pose;
	/* 第一帧为关键帧 */
	if (key_frames_deque_.size() == 0) {
		has_new_key_frame_ = true;
		last_key_pose = laser_odom.pose;
	}

	/* 匹配之后根据距离判断是否需要生成新的关键帧 */
	if (fabs(laser_odom.pose(0, 3) - last_key_pose(0, 3)) + 
		fabs(laser_odom.pose(1, 3) - last_key_pose(1, 3)) +
		fabs(laser_odom.pose(2, 3) - last_key_pose(2, 3)) > key_frame_distance_) {
		
		has_new_key_frame_ = true;
		last_key_pose = laser_odom.pose;
	}

	if (has_new_key_frame_) {
		/* 把关键帧点云存储到硬盘里 */
		std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(key_frames_deque_.size()) + ".pcd";
		pcl::io::savePCDFileBinary(file_path, *cloud_data.cloud_ptr);

		KeyFrame key_frame;
		key_frame.time = laser_odom.time;
		key_frame.index = (unsigned int)key_frames_deque_.size();
		key_frame.pose = laser_odom.pose;
		key_frames_deque_.push_back(key_frame);
		current_key_frame_ = key_frame;

		/*略过gnss_odom的部分*/
		
	}
	return has_new_key_frame_;
}
/**
 * @description: 看是否需要优化
 * @param  {*}
 * @return {*}
 */
bool BackEnd::MaybeOptimized() {
	bool need_optimize = false;

	if (new_loop_cnt_ >= graph_optimizer_config_.optimize_step_with_loop) {
		need_optimize = true;
	}
	if (new_key_frame_cnt_ >= graph_optimizer_config_.optimize_step_with_key_frame) {
		need_optimize = true;
	}

	if (!need_optimize) {
		return false;
	}
	/* 需要优化才会返回true */
	new_loop_cnt_ = 0;
	new_key_frame_cnt_ = 0;

	if (graph_optimizer_ptr_->Optimize()) {
		has_new_optimized_ = true;
	}

	return true;
}

bool BackEnd::SaveOptimizedPose() {
	if (graph_optimizer_ptr_->GetNodeNum() == 0) {
		return false;
	}
	if (!FileManager::CreateFile(optimized_pose_ofs_, trajectory_path_ + "/optimized.txt")) {
		return false;
	}
	/* 将优化位姿放入到optimized_pose_中 */
	graph_optimizer_ptr_->GetOptimizedPose(optimized_pose_);

	for (size_t i = 0; i < optimized_pose_.size(); ++i) {
		SavePose(optimized_pose_ofs_, optimized_pose_.at(i));
	}

	return true;

}

/**
 * @description: 总的更新函数，内部调用其他函数
 * @param  {*}
 * @return {*}
 */
bool BackEnd::Update(const CloudData& cloud_data, const PoseData& laser_odom) {
	ResetParam();

	if (MaybeNewKeyFrame(cloud_data, laser_odom)) {
		// SavePose(ground_truth_ofs_, gnss_pose.pose);
		SavePose(laser_odom_ofs_, laser_odom.pose);
		AddNodeAndEdge();

		if (MaybeOptimized()) {
			SaveOptimizedPose();
		}
	}

	return true;
}

/**
 * @description: 插入闭环
 * @param  {*}
 * @return {*}
 */
bool BackEnd::InsertLoopPose(const LoopPose& loop_pose) {
	if (!graph_optimizer_config_.use_loop_close) {
		return false;
	}

	Eigen::Isometry3d isometry;
	isometry.matrix() = loop_pose.pose.cast<double>();
	graph_optimizer_ptr_->AddSe3Edge(loop_pose.index0, loop_pose.index1, 
		isometry, graph_optimizer_config_.close_loop_noise);

	new_loop_cnt_ ++;

	// LOG(INFO) << "插入闭环：" << loop_pose.index0 << ", " << loop_pose.index1;

	return true;
}

/**
 * @description: 强行优化
 * @param  {*}
 * @return {*}
 */
bool BackEnd::ForceOptimize() {
	if (graph_optimizer_ptr_->Optimize()) {
		has_new_optimized_ = true;
	}

	SaveOptimizedPose();

	return has_new_optimized_;
}

void BackEnd::GetOptimizedKeyFrames(std::deque<KeyFrame>& key_frames_deque) {
	KeyFrame key_frame;
	for (size_t i = 0; i < optimized_pose_.size(); ++i) {
		key_frame.pose = optimized_pose_.at(i);
		key_frame.index = (unsigned int)i;
		key_frames_deque.push_back(key_frame);
	}
}

bool BackEnd::HasNewKeyFrame() {
	return has_new_key_frame_;
}

bool BackEnd::HasNewOptimized() {
	return has_new_optimized_;
}

void BackEnd::GetLatestKeyFrame(KeyFrame& key_frame) {
	key_frame = current_key_frame_;
}

// void BackEnd::GetLatestKeyGNSS(KeyFrame& key_frame) {
// 	// key_frame = current_key_gnss_;
// }

}
